/* *********************************************************************** *
 * project: org.matsim.*
 * AgentInteractionModule.java
 *                                                                         *
 * *********************************************************************** *
 *                                                                         *
 * copyright       : (C) 2007 by the members listed in the COPYING,        *
 *                   LICENSE and WARRANTY file.                            *
 * email           : info at matsim dot org                                *
 *                                                                         *
 * *********************************************************************** *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *   See also COPYING, LICENSE and WARRANTY file                           *
 *                                                                         *
 * *********************************************************************** */
package aiingames.simulation.server.physics;

import java.util.List;

import aiingames.Config;
import aiingames.simulation.server.Server;

import com.vividsolutions.jts.geom.Envelope;
import com.vividsolutions.jts.index.quadtree.Quadtree;


/**
 * Agent interaction forces according to: D. Helbing, I. Farkas, T. Vicsek,
 * Nature 407, 487-490 (2000)
 * 
 * @author laemmel
 * 
 */
public class CircularParticleInteractionModule implements DynamicForceModule {


	private final double quadUpdateInterval = 0.1;
	private double lastQuadUpdate = Double.NEGATIVE_INFINITY;
	protected Quadtree coordsQuad = new Quadtree();
	private Server server;

	//Helbing constants 
	private static final double Bi=0.08;
	private static final double Ai=2000;
	private static final double k = 1.2 * 100000;
	private static final double kappa = 2.4 * 100000;
	
	

	/**
	 * @param floor
	 * @param sceanrio
	 */
	public CircularParticleInteractionModule(Server server) {
		this.server = server;
	}


	@Override
	public void run(Particle particle) {
		updateForces(particle);
	}

	/**
	 * @param agent
	 * @param neighbors
	 */
	/* package */void updateForces(Particle particle) {//, List<Coordinate> neighbors) {
		double fx = 0;
		double fy = 0;

		double minX = particle.getPosition().x - Config.PNeighborhoddRange;
		double maxX = particle.getPosition().x + Config.PNeighborhoddRange;
		double minY = particle.getPosition().y - Config.PNeighborhoddRange;
		double maxY = particle.getPosition().y + Config.PNeighborhoddRange;
		Envelope e = new Envelope(minX, maxX, minY, maxY);
		
		@SuppressWarnings("unchecked")
		List<AgentParticle> l = this.coordsQuad.query(e);
		
		for (Particle other : l) {
			if (other == particle) {
				continue;
			}

			double dist = other.getPosition().distance(particle.getPosition());
			if (dist > Config.PNeighborhoddRange) {
				continue;
			}
			double dx = (particle.getPosition().x - other.getPosition().x) / dist;
			double dy = (particle.getPosition().y - other.getPosition().y) / dist;

			double bounderyDist = (particle.getDiameter()/2+other.getDiameter()/2) - dist;
			double g = bounderyDist > 0 ? bounderyDist : 0;
			
			double tanDvx = (other.getVx() - particle.getVx()) * -dx;
			double tanDvy = (other.getVy() - particle.getVy()) * dy;
			
			double tanX = tanDvx * -dx;
			double tanY = tanDvy * dy;
			double weightRatio = other.getWeight()/particle.getWeight();
			double xc = (Ai * Math.exp((bounderyDist) / Bi) + k*g)* dx+ kappa * g * tanX;
			double yc = (Ai * Math.exp((bounderyDist) / Bi) + k*g)* dy + kappa * g * tanY;

			xc*=weightRatio;
			yc*=weightRatio;
			
			fx += xc;
			fy += yc;
		}
		particle.getForce().incrementX(fx);
		particle.getForce().incrementY(fy);
	}


	/**
	 * 
	 */
	protected void updateAgentQuadtree() {
		this.coordsQuad = new Quadtree();
		for (Particle particle : this.server.getParticles()) {
			Envelope e = new Envelope(particle.getPosition());
			this.coordsQuad.insert(e, particle);
		}

	}

	@Override
	public void update(double time) {
		if (time >= this.lastQuadUpdate + this.quadUpdateInterval) {

			updateAgentQuadtree();

			this.lastQuadUpdate = time;
		}

	}

	@Override
	public void forceUpdate() {
		updateAgentQuadtree();
	}


	@Override
	public void reset() {
		this.lastQuadUpdate = Double.NEGATIVE_INFINITY;
	}

}
